{
  "cells": [
    {
      "attachments": {},
      "cell_type": "markdown",
      "id": "d789b481",
      "metadata": {},
      "source": [
        "# Robotics, Vision & Control 3e: for Python\n",
        "## Chapter 3: Time and Motion\n",
        "\n",
        "Copyright (c) 2021- Peter Corke"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "ce2ca464",
      "metadata": {},
      "outputs": [],
      "source": [
        "try:\n",
        "    from google.colab import output\n",
        "    print('Running on CoLab')\n",
        "    output.enable_custom_widget_manager()\n",
        "    !pip install ipympl\n",
        "    !pip install spatialmath-python\n",
        "    !pip install roboticstoolbox-python>=1.0.2\n",
        "    !pip install --no-deps rvc3python\n",
        "    COLAB = True\n",
        "\n",
        "except ModuleNotFoundError:\n",
        "    COLAB = False\n",
        "\n",
        "from IPython.core.interactiveshell import InteractiveShell\n",
        "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
        "\n",
        "from IPython.display import HTML\n",
        "import urllib.request\n",
        "\n",
        "# add RTB examples folder to the path\n",
        "import sys, os.path\n",
        "import roboticstoolbox as rtb\n",
        "import RVC3\n",
        "# sys.path.append(os.path.join(rtb.__path__[0], 'examples'))\n",
        "sys.path.append(os.path.join(RVC3.__path__[0], 'examples'))\n",
        "\n",
        "# standard imports\n",
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "import math\n",
        "from math import pi\n",
        "np.set_printoptions(\n",
        "    linewidth=120, formatter={\n",
        "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
        "np.random.seed(0)\n",
        "from spatialmath import *\n",
        "from spatialmath.base import *\n",
        "from roboticstoolbox import quintic, trapezoidal, mtraj, mstraj, xplot, ctraj\n",
        "%matplotlib widget\n",
        "import matplotlib.pyplot as plt"
      ]
    },
    {
      "attachments": {},
      "cell_type": "markdown",
      "id": "faa9a9d4",
      "metadata": {},
      "source": [
        "There are some minor code changes compared to the book. These are to support \n",
        "the Matplotlib widget (ipympl) backend.  This allows 3D plots to be rotated\n",
        "so the changes are worthwhile."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "752428f7",
      "metadata": {},
      "source": [
        "# 3.1 Time-Varying Pose\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "f5f01948",
      "metadata": {},
      "source": [
        "## 3.1.3 Transforming Spatial Velocities\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "67e4f8c2",
      "metadata": {},
      "outputs": [],
      "source": [
        "aTb = SE3.Tx(-2) * SE3.Rz(-pi/2) * SE3.Rx(pi/2);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "04057ef6",
      "metadata": {},
      "outputs": [],
      "source": [
        "bV = [1, 2, 3, 4, 5, 6];"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "9e20d026",
      "metadata": {},
      "outputs": [],
      "source": [
        "aJb = aTb.jacob();\n",
        "aJb.shape\n",
        "aV = aJb @ bV"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "abf2e099",
      "metadata": {},
      "outputs": [],
      "source": [
        "aV = aTb.Ad() @ [1, 2, 3, 0, 0, 0]"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "1479d005",
      "metadata": {},
      "outputs": [],
      "source": [
        "aV = aTb.Ad() @ [0, 0, 0, 1, 0, 0]"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "bf746045",
      "metadata": {},
      "outputs": [],
      "source": [
        "aV = aTb.Ad() @ [1, 2, 3, 1, 0, 0]"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "eaac2301",
      "metadata": {},
      "source": [
        "## 3.1.4 Incremental Rotation\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "1a8fcdf0",
      "metadata": {},
      "outputs": [],
      "source": [
        "rotx(0.001)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "73f71233",
      "metadata": {},
      "outputs": [],
      "source": [
        "import time\n",
        "Rexact = np.eye(3); Rapprox = np.eye(3);  # null rotation\n",
        "w = np.array([1, 0, 0]);   # rotation of 1rad/s about x-axis\n",
        "dt = 0.01;                 # time step\n",
        "t0 = time.process_time();\n",
        "for i in range(100):  # exact integration over 100 time steps\n",
        "  Rexact = Rexact @ trexp(skew(w*dt));  # update by composition\n",
        "print(time.process_time() - t0)\n",
        "\n",
        "t0 = time.process_time();\n",
        "for i in range(100):  # approximate integration over 100 time steps\n",
        "  Rapprox = Rapprox + Rapprox @ skew(w*dt);  # update by addition\n",
        "print(time.process_time() - t0)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "b18c487c",
      "metadata": {},
      "outputs": [],
      "source": [
        "np.linalg.det(Rapprox) - 1"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "0f80418c",
      "metadata": {},
      "outputs": [],
      "source": [
        "np.linalg.det(Rexact) - 1"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "d6950b0f",
      "metadata": {},
      "outputs": [],
      "source": [
        "tr2angvec(trnorm(Rexact))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "352ceb0b",
      "metadata": {},
      "outputs": [],
      "source": [
        "tr2angvec(trnorm(Rapprox))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "2f3d51cf",
      "metadata": {},
      "source": [
        "# 3.2 Accelerating Bodies and Reference Frames\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "93cc2ab3",
      "metadata": {},
      "source": [
        "## 3.2.1 Dynamics of Moving Bodies\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "acd64f6b",
      "metadata": {},
      "outputs": [],
      "source": [
        "J = np.array([[ 2, -1, 0],\n",
        "              [-1,  4, 0],\n",
        "              [ 0,  0, 3]]);\n",
        "orientation = UnitQuaternion();  # identity quaternion\n",
        "w = 0.2 * np.array([1, 2, 2]);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "adf9e07e",
      "metadata": {},
      "outputs": [],
      "source": [
        "dt = 0.05;  # time step\n",
        "def update():\n",
        "  global orientation, w\n",
        "  for t in np.arange(0, 10, dt):\n",
        "     wd = -np.linalg.inv(J) @ (np.cross(w, J @ w))  # (3.12)\n",
        "     w += wd * dt\n",
        "     orientation *= UnitQuaternion.EulerVec(w * dt)\n",
        "     yield orientation.R\n",
        "\n",
        "# tranimate(update(), dim=1.5);\n",
        "plotvol3(new=True)\n",
        "HTML(tranimate(update(), dim=1.5, movie=True))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "83eb170e",
      "metadata": {},
      "source": [
        "## 3.2.2 Transforming Forces and Torques\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "bc6b1200",
      "metadata": {},
      "outputs": [],
      "source": [
        "bW = [1, 2, 3, 0, 0, 0];"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "c0fab5e0",
      "metadata": {},
      "outputs": [],
      "source": [
        "aW = aTb.inv().Ad().T @ bW"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "28a58171",
      "metadata": {},
      "source": [
        "# 3.3 Creating Time-Varying Pose\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "90e903cf",
      "metadata": {},
      "source": [
        "## 3.3.1 Smooth One-Dimensional Trajectories\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "acf722f6",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj = quintic(0, 1, np.linspace(0, 1, 50));"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "08d4c3a9",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj.plot();"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "ee3b484b",
      "metadata": {},
      "outputs": [],
      "source": [
        "quintic(0, 1, np.linspace(0, 1, 50), qd0=10, qdf=0);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "69858861",
      "metadata": {},
      "outputs": [],
      "source": [
        "qd = traj.qd;\n",
        "qd.mean() / qd.max()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "2b7e22d1",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj = trapezoidal(0, 1, np.linspace(0, 1, 50));\n",
        "traj.plot();"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "a6641c50",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj.qd.max()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "2451dd66",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj1_2 = trapezoidal(0, 1, np.linspace(0, 1, 50), V=1.2);\n",
        "traj2_0 = trapezoidal(0, 1, np.linspace(0, 1, 50), V=2);"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "01dbb8e5",
      "metadata": {},
      "source": [
        "## 3.3.2 Multi-Axis Trajectories\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "c3b8ce3b",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj = mtraj(trapezoidal, [0, 2], [1, -1], 50);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "27da3e80",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj.plot();"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "20e1ff3d",
      "metadata": {},
      "outputs": [],
      "source": [
        "T = SE3.Rand()\n",
        "q = np.hstack([T.t, T.rpy()])"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "23d16f5f",
      "metadata": {},
      "source": [
        "## 3.3.3 Multi-Segment Trajectories\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "88cb19b2",
      "metadata": {},
      "outputs": [],
      "source": [
        "via = SO2(30, unit=\"deg\") * np.array([[-1, 1, 1, -1, -1], [1, 1, -1, -1, 1]]);\n",
        "traj0 = mstraj(via.T, dt=0.2, tacc=0, qdmax=[2, 1]);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "64782a1f",
      "metadata": {},
      "outputs": [],
      "source": [
        "# xplot(traj0.q[:, 0], traj0.q[:, 1], color=\"red\");\n",
        "plt.plot(traj0.q[:, 0], traj0.q[:, 1], color=\"red\");"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "57dcefd2",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj2 = mstraj(via.T, dt=0.2, tacc=2, qdmax=[2, 1]);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "25798df6",
      "metadata": {},
      "outputs": [],
      "source": [
        "len(traj0), len(traj2)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "b82e625e",
      "metadata": {},
      "source": [
        "## 3.3.4 Interpolation of Orientation in 3D\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "7b1908c1",
      "metadata": {},
      "outputs": [],
      "source": [
        "R0 = SO3.Rz(-1) * SO3.Ry(-1);\n",
        "R1 = SO3.Rz(1) * SO3.Ry(1);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "9213af63",
      "metadata": {},
      "outputs": [],
      "source": [
        "rpy0 = R0.rpy(); rpy1 = R1.rpy();"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "42373ac0",
      "metadata": {},
      "outputs": [],
      "source": [
        "traj = mtraj(quintic, rpy0, rpy1, 50);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "26ee84b0",
      "metadata": {},
      "outputs": [],
      "source": [
        "pose = SO3.RPY(traj.q);\n",
        "len(pose)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "d62a1caf",
      "metadata": {},
      "outputs": [],
      "source": [
        "# pose.animate()\n",
        "HTML(pose.animate(movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "23553efc",
      "metadata": {},
      "outputs": [],
      "source": [
        "q0 = UnitQuaternion(R0); q1 = UnitQuaternion(R1);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "9c87d883",
      "metadata": {},
      "outputs": [],
      "source": [
        "qtraj = q0.interp(q1, 50);\n",
        "len(qtraj)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "17fb4aa8",
      "metadata": {},
      "outputs": [],
      "source": [
        "# qtraj.animate()\n",
        "plotvol3(new=True)  # for matplotlib/widget\n",
        "HTML(qtraj.animate(movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "95a17f6e",
      "metadata": {},
      "source": [
        "### 3.3.4.1 Direction of Rotation\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "48d0377e",
      "metadata": {},
      "outputs": [],
      "source": [
        "q0 = UnitQuaternion.Rz(-2); q1 = UnitQuaternion.Rz(2);\n",
        "q = q0.interp(q1, 50);\n",
        "# q.animate(dim=1.])\n",
        "plotvol3(new=True)  # for matplotlib/widget\n",
        "HTML(q.animate(movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "afb8b452",
      "metadata": {},
      "outputs": [],
      "source": [
        "q = q0.interp(q1, 50, shortest=True);\n",
        "# q.animate()\n",
        "plotvol3(new=True)  # for matplotlib/widget\n",
        "HTML(q.animate(movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "f4cc2d00",
      "metadata": {},
      "source": [
        "## 3.3.5 Cartesian Motion in 3D\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "d311c94a",
      "metadata": {},
      "outputs": [],
      "source": [
        "T0 = SE3.Trans([0.4, 0.2, 0]) * SE3.RPY(0, 0, 3);\n",
        "T1 = SE3.Trans([-0.4, -0.2, 0.3]) * SE3.RPY(-pi/4, pi/4, -pi/2);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "3c077d85",
      "metadata": {},
      "outputs": [],
      "source": [
        "T0.interp(T1, 0.5)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "c9436e23",
      "metadata": {},
      "outputs": [],
      "source": [
        "Ts = T0.interp(T1, 51);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "8cb266c2",
      "metadata": {},
      "outputs": [],
      "source": [
        "len(Ts)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "5adc6e0a",
      "metadata": {},
      "outputs": [],
      "source": [
        "# Ts.animate()\n",
        "plotvol3(new=True)  # for matplotlib/widget\n",
        "HTML(Ts.animate(movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "e58d1854",
      "metadata": {},
      "outputs": [],
      "source": [
        "Ts[25]"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "6f8ae326",
      "metadata": {},
      "outputs": [],
      "source": [
        "P = Ts.t;\n",
        "P.shape"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "370e7978",
      "metadata": {},
      "outputs": [],
      "source": [
        "xplot(P, labels=\"x y z\");"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "ab763d64",
      "metadata": {},
      "outputs": [],
      "source": [
        "rpy = Ts.rpy();\n",
        "xplot(rpy, labels=\"roll pitch yaw\");"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "9314c598",
      "metadata": {},
      "outputs": [],
      "source": [
        "Ts = T0.interp(T1, trapezoidal(0, 1, 50).q);"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "8667bc56",
      "metadata": {},
      "outputs": [],
      "source": [
        "Ts = ctraj(T0, T1, 50);"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "9eebfeb2",
      "metadata": {},
      "source": [
        "# 3.4 Application: Inertial Navigation\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "93ac0521",
      "metadata": {},
      "source": [
        "## 3.4.1 Gyroscopes\n"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c8b681b9",
      "metadata": {},
      "source": [
        "### 3.4.1.2 Estimating Orientation\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "4fe1e894",
      "metadata": {},
      "outputs": [],
      "source": [
        "from imu_data import IMU\n",
        "true, _ = IMU()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "42707569",
      "metadata": {},
      "outputs": [],
      "source": [
        "orientation = UnitQuaternion();  # identity quaternion"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "68b0a5d7",
      "metadata": {},
      "outputs": [],
      "source": [
        "for w in true.omega[:-1]:\n",
        "  next = orientation[-1] @ UnitQuaternion.EulerVec(w * true.dt);\n",
        "  orientation.append(next);\n",
        "len(orientation)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "24badf62",
      "metadata": {},
      "outputs": [],
      "source": [
        "# orientation.animate(time=true.t)\n",
        "HTML(orientation.animate(time=true.t, movie=True, dim=1.5))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "2673cd14",
      "metadata": {},
      "outputs": [],
      "source": [
        "xplot(true.t, orientation.rpy(), labels=\"roll pitch yaw\");"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "f358dff8",
      "metadata": {},
      "source": [
        "## 3.4.4 Inertial Sensor Fusion\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "a7d89d8f",
      "metadata": {},
      "outputs": [],
      "source": [
        "from imu_data import IMU\n",
        "true, imu = IMU()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "30468005",
      "metadata": {},
      "outputs": [],
      "source": [
        "q = UnitQuaternion();\n",
        "for wm in imu.gyro[:-1]:\n",
        "  q.append(q[-1] @ UnitQuaternion.EulerVec(wm * imu.dt))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "2c3f48a5",
      "metadata": {},
      "outputs": [],
      "source": [
        "xplot(true.t, q.angdist(true.orientation), color=\"red\");"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "ab939014",
      "metadata": {},
      "outputs": [],
      "source": [
        "kI = 0.2; kP = 1;\n",
        "b = np.zeros(imu.gyro.shape);\n",
        "qcf = UnitQuaternion();\n",
        "data = zip(imu.gyro[:-1], imu.accel[:-1], imu.magno[:-1]);\n",
        "for k, (wm, am, mm) in enumerate(data):\n",
        "  qi = qcf[-1].inv()\n",
        "  sR = np.cross(am, qi * true.g) + np.cross(mm, qi * true.B)\n",
        "  wp = wm - b[k,:] + kP * sR\n",
        "  qcf.append(qcf[k] @ UnitQuaternion.EulerVec(wp * imu.dt))\n",
        "  b[k+1,:] = b[k,:] - kI * sR * imu.dt"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "36c71a2c",
      "metadata": {},
      "outputs": [],
      "source": [
        "xplot(true.t, qcf.angdist(true.orientation), color=\"blue\");"
      ]
    }
  ],
  "metadata": {
    "jupytext": {
      "cell_metadata_filter": "-all",
      "main_language": "python",
      "notebook_metadata_filter": "-all"
    },
    "kernelspec": {
      "display_name": "dev",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.10.9"
    },
    "vscode": {
      "interpreter": {
        "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
      }
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
